copp\copp\copp3/
formulation.rs

1//! Problem data models and builders for TOPP3/COPP3.
2//!
3//! # Notation policy (math + code)
4//! To help users map paper notation to API fields without ambiguity,
5//! this module follows a dual notation style:
6//! - mathematical definition uses KaTeX, e.g. $a_k = \dot{s}_k^2$ and $b_k = \ddot{s}_k$;
7//! - discrete implementation uses code symbols, e.g. `a[k]`, `b[k]`, `s[k]`.
8//!
9//! # Reference
10//! Wang, Y., Hu, C., Li, Y., Yu, J., Yan, J., Liang, Y., & Jin, Z. (2026).
11//! Online time-optimal trajectory planning along parametric toolpaths with strict constraint
12//! satisfaction and certifiable feasibility guarantee.
13//! *International Journal of Machine Tools and Manufacture*, 215, 104355.
14//! <https://doi.org/10.1016/j.ijmachtools.2025.104355>
15//!
16//! # Stationary-boundary modeling note
17//! This module exposes `num_stationary_max=(start,end)` on builders as a **user input upper
18//! bound** and derives effective `num_stationary` during `build_with_linearization()`.
19//! For typical users, `num_stationary_max=(1,1)` is recommended.
20//!
21//! For stationary boundaries (`a=b=0`), using zero stationary intervals is allowed, but the
22//! boundary-time model can become ill-conditioned because the regular
23//! $c=\frac{\dddot{s}}{\dot{s}}$-constant formulation degenerates near zero speed.
24//! A short boundary neighborhood modeled by $\dddot{s}$-constant stationary intervals is the
25//! practical remedy.
26//!
27//! For online/windowed planning, `num_stationary_max` can be asymmetric. A practical example is
28//! [test_mercedes_benz_mold](../../../../tests/test_real_cnc/test_real_cnc.rs), where window-end
29//! stationary allowance is intentionally relaxed for intermediate windows.
30
31use crate::copp::CoppObjective;
32use crate::copp::constraints::Constraints;
33use crate::diag::{CoppError, check_boundary_state_copp3_valid, check_s_interval_valid};
34use crate::robot::robot_core::{Robot, RobotBasic, RobotTorque};
35use itertools::izip;
36
37const DEFAULT_A_LINEARIZATION_FLOOR: f64 = 1E-10;
38const DEFAULT_NUM_STATIONARY_MAX: (usize, usize) = (1, 1);
39
40#[inline(always)]
41fn determine_num_stationary_side(a: f64, b: f64, num_stationary_max: usize) -> usize {
42    if a.abs() < f64::EPSILON && b.abs() < f64::EPSILON {
43        num_stationary_max
44    } else {
45        0
46    }
47}
48
49#[inline(always)]
50fn determine_num_stationary_pair(
51    a_boundary: (f64, f64),
52    b_boundary: (f64, f64),
53    num_stationary_max: (usize, usize),
54) -> (usize, usize) {
55    (
56        determine_num_stationary_side(a_boundary.0, b_boundary.0, num_stationary_max.0),
57        determine_num_stationary_side(a_boundary.1, b_boundary.1, num_stationary_max.1),
58    )
59}
60
61/// Prepared TOPP3 problem view.
62///
63/// # Method identity
64/// This is the **read-only runtime view** consumed by TOPP3/COPP3 solvers after
65/// third-order constraints have been linearized.
66///
67/// # Important invariant
68/// The builder precomputes linearized jerk buffers in `Constraints`:
69/// - `jerk_a_linear`
70/// - `jerk_max_linear`
71///
72/// and this object subsequently holds only `&Constraints` (non-mutable view).
73///
74/// # Why this works
75/// Original third-order inequality includes a nonlinear denominator term:
76/// $$
77/// \sqrt{a(s)}\left(g\_a(s) a(s) + g\_b(s) b(s) + g\_c(s) c(s) + g\_d(s)\right) \le g\_{\text{max}}(s).
78/// $$
79///
80/// Around reference `a_linearization`, it is approximated into affine form:
81/// $$
82/// \left(g\_a(s) + \frac{g\_{\text{max}}(s)}{2a_{lin}^{3/2}}\right)a + g\_b(s) b + g\_c(s) c
83/// \le \frac{3g\_{\text{max}}(s)}{2a_{lin}^{1/2}} - g\_d(s).
84/// $$
85/// where $a_{lin}$ corresponds to code input `a_linearization[k]`.
86///
87/// The affine coefficients are stored into those two buffers for downstream LP/SOCP/RA use.
88pub struct Topp3Problem<'a> {
89    pub(crate) constraints: &'a Constraints,
90    pub(crate) idx_s_start: usize,
91    pub(crate) a_linearization: &'a [f64],
92    pub(crate) a_boundary: (f64, f64),
93    pub(crate) b_boundary: (f64, f64),
94    /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
95    /// boundary conditions and `num_stationary_max`.
96    pub(crate) num_stationary: (usize, usize),
97}
98
99/// Builder for [`Topp3Problem`], including optional in-build linearization.
100///
101/// # Side effect notice
102/// `build_with_linearization()` updates cached affine linearization data inside `Constraints`.
103/// Raw jerk constraints remain unchanged.
104pub struct Topp3ProblemBuilder<'a> {
105    /// Mutable constraint storage used to build linearized TOPP3 problem data.
106    pub constraints: &'a mut Constraints,
107    /// Start station index of the optimization interval.
108    pub idx_s_start: usize,
109    /// Reference profile `a[k]` used to linearize third-order constraints.
110    pub a_linearization: &'a [f64],
111    /// Boundary values of `a=(a_start, a_final)`.
112    pub a_boundary: (f64, f64),
113    /// Boundary values of `b=(b_start, b_final)`.
114    pub b_boundary: (f64, f64),
115    /// User-input upper bound of stationary intervals at (start, end).
116    pub num_stationary_max: (usize, usize),
117    /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
118    ///
119    /// Effective usage in linearization is:
120    /// $$
121    /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
122    /// $$
123    /// Discrete code form:
124    /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
125    ///
126    /// More details are available in the [`Topp3Problem`] documentation.
127    pub a_linearization_floor: f64,
128}
129
130impl<'a> Topp3ProblemBuilder<'a> {
131    /// Create a TOPP3 builder with required fields.
132    ///
133    /// Defaults:
134    /// - `num_stationary_max = (1, 1)`
135    /// - `a_linearization_floor = 1E-10`
136    pub fn new<M: RobotBasic>(
137        robot: &'a mut Robot<M>,
138        idx_s_start: usize,
139        a_linearization: &'a [f64],
140        a_boundary: (f64, f64),
141        b_boundary: (f64, f64),
142    ) -> Self {
143        Self {
144            constraints: &mut robot.constraints,
145            idx_s_start,
146            a_linearization,
147            a_boundary,
148            b_boundary,
149            num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
150            a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
151        }
152    }
153
154    /// Create a TOPP3 builder with required fields.
155    ///
156    /// Defaults:
157    /// - `num_stationary_max = (1, 1)`
158    /// - `a_linearization_floor = 1E-10`
159    pub fn with_constraint(
160        constraints: &'a mut Constraints,
161        idx_s_start: usize,
162        a_linearization: &'a [f64],
163        a_boundary: (f64, f64),
164        b_boundary: (f64, f64),
165    ) -> Self {
166        Self {
167            constraints,
168            idx_s_start,
169            a_linearization,
170            a_boundary,
171            b_boundary,
172            num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
173            a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
174        }
175    }
176
177    /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
178    ///
179    /// See module-level **Stationary-boundary modeling note** for guidance.
180    #[inline]
181    pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
182        self.num_stationary_max = (num_stationary_max, num_stationary_max);
183        self
184    }
185
186    /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
187    ///
188    /// See module-level **Stationary-boundary modeling note** for guidance.
189    #[inline]
190    pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
191        self.num_stationary_max = num_stationary_max;
192        self
193    }
194
195    /// Set denominator floor used in linearization.
196    #[inline]
197    pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
198        self.a_linearization_floor = floor;
199        self
200    }
201
202    /// Build a TOPP3 problem and linearize third-order constraints in one step.
203    ///
204    /// This validates boundaries/interval/floor first, then writes linearized jerk buffers.
205    pub fn build_with_linearization(self) -> Result<Topp3Problem<'a>, CoppError> {
206        check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
207        if self.a_linearization.is_empty() {
208            return Err(CoppError::InvalidInput(
209                "Topp3ProblemBuilder::build_with_linearization".into(),
210                "a_linearization cannot be empty".into(),
211            ));
212        }
213        let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
214        check_s_interval_valid(
215            "Topp3ProblemBuilder::build_with_linearization",
216            self.idx_s_start,
217            idx_s_final,
218        )?;
219        if self.a_linearization_floor <= 0.0 {
220            return Err(CoppError::InvalidInput(
221                "Topp3ProblemBuilder::build_with_linearization".into(),
222                format!(
223                    "a_linearization_floor must be positive, got {}",
224                    self.a_linearization_floor
225                ),
226            ));
227        }
228
229        self.constraints
230            .linearize_constraint_3order_with_floor(
231                self.a_linearization,
232                self.idx_s_start,
233                self.a_linearization_floor,
234            )
235            .map_err(|e| {
236                CoppError::InvalidInput(
237                    "Topp3ProblemBuilder::build_with_linearization".into(),
238                    format!("linearize_constraint_3order failed: {e}"),
239                )
240            })?;
241
242        let num_stationary = determine_num_stationary_pair(
243            self.a_boundary,
244            self.b_boundary,
245            self.num_stationary_max,
246        );
247
248        Ok(Topp3Problem {
249            constraints: &*self.constraints,
250            idx_s_start: self.idx_s_start,
251            a_linearization: self.a_linearization,
252            a_boundary: self.a_boundary,
253            b_boundary: self.b_boundary,
254            num_stationary,
255        })
256    }
257}
258
259/// The problem of COPP3.
260/// # Arguments  
261/// * `robot` - A robot with torque implemented, which defines the constraints and dynamic of the problem.  
262/// * `objectives` - Objectives for COPP3 optimization.     
263/// * `idx_s_start` - The starting index along the path (reached).  
264/// * `a_linearization` - Linearization reference profile for third-order constraints.
265/// * `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.  
266/// * `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.  
267/// * `num_stationary=(start,end)` - Effective stationary intervals derived in `build_with_linearization()`.  
268pub struct Copp3Problem<'a, M: RobotTorque> {
269    pub(crate) robot: &'a mut Robot<M>,
270    pub(crate) objectives: &'a [CoppObjective<'a>],
271    pub(crate) idx_s_start: usize,
272    pub(crate) a_linearization: &'a [f64],
273    pub(crate) a_boundary: (f64, f64),
274    pub(crate) b_boundary: (f64, f64),
275    /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
276    /// boundary conditions and `num_stationary_max`.
277    pub(crate) num_stationary: (usize, usize),
278}
279
280/// Builder for [`Copp3Problem`].
281pub struct Copp3ProblemBuilder<'a, M: RobotTorque> {
282    /// A robot with torque implemented, which defines the constraints and dynamic of the problem.
283    pub robot: &'a mut Robot<M>,
284    /// Objectives for COPP3 optimization.
285    pub objectives: &'a [CoppObjective<'a>],
286    /// The starting index along the path (reached).
287    pub idx_s_start: usize,
288    /// Linearization reference profile for third-order constraints.
289    pub a_linearization: &'a [f64],
290    /// `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
291    pub a_boundary: (f64, f64),
292    /// `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
293    pub b_boundary: (f64, f64),
294    /// User-input upper bound of stationary intervals at (start, end).
295    pub num_stationary_max: (usize, usize),
296}
297
298impl<'a, M: RobotTorque> Copp3ProblemBuilder<'a, M> {
299    /// Create a COPP3 builder with required fields.
300    ///
301    /// Default:
302    /// - `num_stationary_max = (1, 1)`
303    pub fn new(
304        robot: &'a mut Robot<M>,
305        objectives: &'a [CoppObjective<'a>],
306        idx_s_start: usize,
307        a_linearization: &'a [f64],
308        a_boundary: (f64, f64),
309        b_boundary: (f64, f64),
310    ) -> Self {
311        Self {
312            robot,
313            objectives,
314            idx_s_start,
315            a_linearization,
316            a_boundary,
317            b_boundary,
318            num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
319        }
320    }
321
322    /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
323    ///
324    /// See module-level **Stationary-boundary modeling note** for guidance.
325    #[inline]
326    pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
327        self.num_stationary_max = (num_stationary_max, num_stationary_max);
328        self
329    }
330
331    /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
332    ///
333    /// See module-level **Stationary-boundary modeling note** for guidance.
334    #[inline]
335    pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
336        self.num_stationary_max = num_stationary_max;
337        self
338    }
339
340    /// Build a validated COPP3 problem and linearize third-order constraints in one step.
341    pub fn build_with_linearization(self) -> Result<Copp3Problem<'a, M>, CoppError> {
342        check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
343        if self.a_linearization.is_empty() {
344            return Err(CoppError::InvalidInput(
345                "Copp3ProblemBuilder::build_with_linearization".into(),
346                "a_linearization cannot be empty".into(),
347            ));
348        }
349        let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
350        check_s_interval_valid(
351            "Copp3ProblemBuilder::build_with_linearization",
352            self.idx_s_start,
353            idx_s_final,
354        )?;
355        self.robot
356            .constraints
357            .check_s_in_bounds(self.idx_s_start, self.a_linearization.len())?;
358
359        self.robot
360            .constraints
361            .linearize_constraint_3order_with_floor(
362                self.a_linearization,
363                self.idx_s_start,
364                DEFAULT_A_LINEARIZATION_FLOOR,
365            )
366            .map_err(|e| {
367                CoppError::InvalidInput(
368                    "Copp3ProblemBuilder::build_with_linearization".into(),
369                    format!("linearize_constraint_3order failed: {e}"),
370                )
371            })?;
372
373        let num_stationary = determine_num_stationary_pair(
374            self.a_boundary,
375            self.b_boundary,
376            self.num_stationary_max,
377        );
378
379        Ok(Copp3Problem {
380            robot: self.robot,
381            objectives: self.objectives,
382            idx_s_start: self.idx_s_start,
383            a_linearization: self.a_linearization,
384            a_boundary: self.a_boundary,
385            b_boundary: self.b_boundary,
386            num_stationary,
387        })
388    }
389}
390
391impl<'a, M: RobotTorque> Copp3Problem<'a, M> {
392    /// Update linearization profile.
393    #[inline]
394    pub fn set_a_linearization(&mut self, a_linearization: &'a [f64]) {
395        self.a_linearization = a_linearization;
396    }
397
398    /// Backward-compatible alias for `set_a_linearization`.
399    #[inline]
400    pub fn set_a_linear(&mut self, a_linearization: &'a [f64]) {
401        self.set_a_linearization(a_linearization);
402    }
403
404    /// Update objective list.
405    #[inline]
406    pub fn set_objective(&mut self, objective: &'a [CoppObjective<'a>]) {
407        self.objectives = objective;
408    }
409
410    /// Convert to the TOPP3 view that shares interval/boundary/linearization fields.
411    pub fn as_topp3_problem(&self) -> Topp3Problem<'_> {
412        Topp3Problem {
413            constraints: &self.robot.constraints,
414            idx_s_start: self.idx_s_start,
415            a_linearization: self.a_linearization,
416            a_boundary: self.a_boundary,
417            b_boundary: self.b_boundary,
418            num_stationary: self.num_stationary,
419        }
420    }
421}
422
423/// Get the weight of `a` for value function.  
424/// Time loss = \sum_{k=0}^n weight_a[k] / sqrt(a[k]) \approx Time
425pub(crate) fn get_weight_a_topp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
426    let n = s.len() - 1;
427    let mut weight_a = vec![0.0; s.len()];
428    if num_stationary.0 > 0 {
429        weight_a[num_stationary.0] =
430            0.5 * (5.0 * s[num_stationary.0] + s[num_stationary.0 + 1] - 6.0 * s[0]);
431    }
432    if num_stationary.1 > 0 {
433        weight_a[n - num_stationary.1] =
434            0.5 * (6.0 * s[n] - 5.0 * s[n - num_stationary.1] - s[n - num_stationary.1 - 1]);
435    }
436    weight_a
437        .iter_mut()
438        .skip(1)
439        .zip(s.windows(3))
440        .skip(num_stationary.0)
441        .take(n - num_stationary.0 - num_stationary.1 - 1)
442        .for_each(|(w_a, s_slice)| {
443            *w_a = 0.5 * (s_slice[2] - s_slice[0]);
444        });
445
446    weight_a
447}
448
449/// Get the weight of `a` for value function.
450/// Loss = weight[0] * loss_average_left / sqrt(a[num_stationary.0]) + weight[n] * loss_average_right / sqrt(a[n - num_stationary.1]) + \sum_{k=num_stationary.0}^{n-num_stationary.1-1} weight_a[k] * loss[k] / sqrt(a[k])
451pub(crate) fn get_weight_a_copp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
452    let n = s.len() - 1;
453    let mut weight_a = vec![0.0; s.len()];
454    if num_stationary.0 > 0 {
455        let s_n1 = s[num_stationary.0];
456        weight_a[num_stationary.0] = 0.5 * (s[num_stationary.0 + 1] - s_n1);
457        weight_a[0] = 3.0 * (s_n1 - s[0]);
458    }
459    if num_stationary.1 > 0 {
460        let s_n2 = s[n - num_stationary.1];
461        weight_a[n - num_stationary.1] = 0.5 * (s_n2 - s[n - num_stationary.1 - 1]);
462        weight_a[n] = 3.0 * (s[n] - s_n2);
463    }
464    weight_a
465        .iter_mut()
466        .skip(1)
467        .zip(s.windows(3))
468        .skip(num_stationary.0)
469        .take(n - num_stationary.0 - num_stationary.1 - 1)
470        .for_each(|(w_a, s_slice)| {
471            *w_a = 0.5 * (s_slice[2] - s_slice[0]);
472        });
473
474    weight_a
475}
476
477/// Get the `a` and `b` profiles for stationary intervals.
478pub(crate) fn set_ab_stationary_topp3<const START: bool>(
479    s: &[f64],
480    a: &mut [f64],
481    b: &mut [f64],
482    a_stationary: f64,
483    num_stationary: usize,
484) {
485    if num_stationary == 0 {
486        return;
487    }
488    if START {
489        let s_start = s[0];
490        let ds_start = s[num_stationary] - s_start;
491        *a.first_mut().unwrap() = 0.0;
492        *b.first_mut().unwrap() = 0.0;
493        a[num_stationary] = a_stationary;
494        b[num_stationary] = a_stationary / (1.5 * ds_start);
495        if num_stationary > 1 {
496            for (a_k, b_k, &s_k) in izip!(a.iter_mut(), b.iter_mut(), s.iter())
497                .skip(1)
498                .take(num_stationary - 1)
499            {
500                let dsk_start = s_k - s_start;
501                let mut alpha = dsk_start / ds_start;
502                alpha *= alpha.cbrt();
503                *a_k = a_stationary * alpha;
504                *b_k = *a_k / (1.5 * dsk_start);
505            }
506        }
507    } else {
508        let &s_final = s.last().unwrap();
509        let n = s.len() - 1;
510        let ds_final = s[n - num_stationary] - s_final;
511        *a.last_mut().unwrap() = 0.0;
512        *b.last_mut().unwrap() = 0.0;
513        a[a.len() - 1 - num_stationary] = a_stationary;
514        b[b.len() - 1 - num_stationary] = a_stationary / (1.5 * ds_final);
515        if num_stationary > 1 {
516            for (a_k, b_k, &s_k) in izip!(a.iter_mut().rev(), b.iter_mut().rev(), s.iter().rev())
517                .skip(1)
518                .take(num_stationary - 1)
519            {
520                let dsk_final = s_k - s_final;
521                let mut alpha = dsk_final / ds_final;
522                alpha *= alpha.cbrt();
523                *a_k = a_stationary * alpha;
524                *b_k = *a_k / (1.5 * dsk_final);
525            }
526        }
527    }
528}
529
530#[cfg(test)]
531mod tests {
532    use super::determine_num_stationary_pair;
533
534    #[test]
535    fn test_determine_num_stationary_pair_respects_boundary_state() {
536        let pair = determine_num_stationary_pair((0.0, 0.0), (0.0, 0.0), (2, 3));
537        assert_eq!(pair, (2, 3));
538
539        let pair = determine_num_stationary_pair((1.0, 0.0), (0.0, 0.0), (2, 3));
540        assert_eq!(pair, (0, 3));
541
542        let pair = determine_num_stationary_pair((0.0, 1.0), (0.0, 1.0), (2, 3));
543        assert_eq!(pair, (2, 0));
544    }
545}
copp\copp\copp3/formulation.rs

copp\copp\copp3/
formulation.rs
